#ifndef PRIORITY_QUEUE_HPP
#define PRIORITY_QUEUE_HPP

#include "raylib.h"
#include "raymath.h"
#include <vector>
#include <algorithm>
#include <functional>

class PriorityQueue {
private:
std::vector<Vector2> heap;

void heapifyDown(int idx) {
int size = heap.size();
int largest = idx;
int left = 2 * idx + 1;
int right = 2 * idx + 2;

if (left < size && Vector2Length(heap[left]) > Vector2Length(heap[largest])) {
largest = left;
}
if (right < size && Vector2Length(heap[right]) > Vector2Length(heap[largest])) {
largest = right;
}
if (largest != idx) {
std::swap(heap[idx], heap[largest]);
heapifyDown(largest);
}
}

void heapifyUp(int idx) {
while (idx != 0 && Vector2Length(heap[(idx - 1) / 2]) < Vector2Length(heap[idx])) {
std::swap(heap[idx], heap[(idx - 1) / 2]);
idx = (idx - 1) / 2;
}
}

public:
void insert(Vector2 point) {
heap.push_back(point);
int current = heap.size() - 1;
heapifyUp(current);
}

Vector2 extractMax() {
Vector2 max = heap[0];
heap[0] = heap.back();
heap.pop_back();
heapifyDown(0);
return max;
}

bool isEmpty() const {
return heap.empty();
}

void clear() {
heap.clear();
}
};

#endif // 